
#include "contact_detection.h"
using namespace std;
contact_detection_s contact_detection;


void contact_detection_s::configuration()
{
    noise_phase=0.998;//设置噪声
    noise_force=0*0.841;
    noise_height=0.930;
    
    mean_force=20;//触地力的阀值
    variance_phase=0.05;
    variance_force=1;
    variance_height=0.05;
    
    probability_threshold=0.5;
    
    A=Eigen::MatrixXd::Zero(4,4);
    B=Eigen::MatrixXd::Identity(4,4);
    K=Eigen::MatrixXd::Zero(4,8);
    H=Eigen::MatrixXd::Identity(8,4);
    H.block(4,0,4,4)=Eigen::MatrixXd::Identity(4,4);
    x=Eigen::MatrixXd::Zero(4,1);
    x_=Eigen::MatrixXd::Zero(4,1);
    u=Eigen::MatrixXd::Zero(4,1);
    z1=Eigen::MatrixXd::Zero(4,1);
    z2=Eigen::MatrixXd::Zero(4,1);
    z=Eigen::MatrixXd::Zero(8,1);
    sigma=Eigen::MatrixXd::Zero(4,4);
    sigma_=Eigen::MatrixXd::Zero(4,4);
    sigma_w=noise_phase*Eigen::MatrixXd::Identity(4,4);
    sigma_v1=noise_force*Eigen::MatrixXd::Identity(4,4);
    sigma_v2=noise_height*Eigen::MatrixXd::Identity(4,4);
    sigma_v=Eigen::MatrixXd::Zero(8,8);
    sigma_v.block(0,0,4,4)=sigma_v1;
    sigma_v.block(4,4,4,4)=sigma_v2;
    
    I=Eigen::MatrixXd::Identity(4,4);
    
    
    #ifdef simulation_mode
    for(int i=0;i<4;i++)
    {
        touchsensor[i]=robot_global->getTouchSensor(touchsensor_name[i]);
        touchsensor[i]->enable(SIMULATION_STEP_DURATION);
        
    }
    #endif
    
}
void contact_detection_s::periodic_task()
{
    this->get_necessary_data();
    this->calculate_probability();//计算触地概率
    this->kalman_fusion();//卡尔曼滤波
    this->calculate_result(); //得出结果
    
    #ifdef simulation_mode
    for(int i=0;i<4;i++)
    contact_touchsensor[i]=touchsensor[i]->getValue();
    #endif
    
}

void contact_detection_s::get_necessary_data()
{
    for(int i=0;i<4;i++)
    {
       // force[i]=leg.get_force_get(i,2);
        //phase[i]=;
        //height[i]=;
    }
    
}
void contact_detection_s::calculate_probability()
{
    for(int i=0;i<4;i++)
    {
        p_force[i]=(1+erf((force[i]-mean_force)/(variance_force*sqrt(2))))/2;
        z1(i,0)=p_force[i];
    }
    z.block(0,0,4,1)=z1;
    z.block(4,0,4,1)=z2;
}
void contact_detection_s::kalman_fusion()
{
    x_=A*x+B*u;
    sigma_=A*sigma*A.transpose()+sigma_w;
    Eigen::MatrixXd H_t=H.transpose();
    Eigen::MatrixXd temp=H*sigma_*H_t+sigma_v;
    
    temp=temp.inverse();
    K=sigma_*H_t*temp;
    x=x_+K*(z-H*x_);
    sigma=(I-K*H)*sigma_;
}

void contact_detection_s::calculate_result()
{
    for(int i=0;i<4;i++)
    {
        if(x(i,0)>probability_threshold)contact[i]=1;
        else contact[i]=0;
        
        
    }
}
 